/*
 * bsp_uart.h
 *
 *  Created on: Mar 1, 2024
 *      Author: SDJX
 */

#include "bsp.h"
#include "stdbool.h"
#include "math.h"
#include "stdlib.h"
#include "string.h"
#include "stdbool.h"

extern bool RxSucceeflag;
extern float aX,aY,aZ,RollX,PitchY,YawZ;

// LED显示当前运行状态，每10毫秒调用一次，LED灯每200毫秒闪烁一次。
// The LED displays the current operating status, which is invoked every 10 milliseconds, and the LED blinks every 200 milliseconds.  
static void Bsp_Led_Show_State_Handle(void)
{
	static uint8_t led_count = 0;
	led_count++;
	if (led_count > 20)
	{
		led_count = 0;
		LED_TOGGLE();
	}
}

#define Forward 0		//前进
#define Right_turn 1	//右转
#define Left_turn 2		//左转
#define Backward 3		//后退
#define Left 4			//左移
#define Right 5			//右移

void USART_Send(uint8_t *data,uint8_t len)
{
	for(int ii=0;ii<len;ii++)
	{
		HAL_UART_Transmit(&huart1,&data[ii],1,1000);
	}
}

void JY61P_Z_reset(){//陀螺仪z轴复位
	unsigned char prt[16]={0};
	prt[0]=0xff;
	prt[1]=0xaa;
	prt[2]=0x69;
	prt[3]=0x88;
	prt[4]=0xb5;
	prt[5]=0;
	USART_Send(prt,5);
	HAL_Delay(200);//延时200ms

	prt[0]=0xff;
	prt[1]=0xaa;
	prt[2]=0x01;
	prt[3]=0x08;
	prt[4]=0x00;
	prt[5]=0;
	USART_Send(prt,5);
	HAL_Delay(3000);//延时3s

	prt[0]=0xff;
	prt[1]=0xaa;
	prt[2]=0x00;
	prt[3]=0x00;
	prt[4]=0x00;
	prt[5]=0;
	USART_Send(prt,5);

}

// The peripheral device is initialized  外设设备初始化
void Bsp_Init(void)
{
	//Beep_On_Time(50);
	USART1_Init();
	Motor_Init();
	Encoder_Init();
	PID_Param_Init();
	HAL_InitTick(1);
}

int car_state = 0;

int encoder[4] = {0};
int show_encoder = 0;
int speed_2 = 0;

int time_cnt_1 = 0;		//间隔时间
uint8_t st = 0;			//转向
uint8_t Run_C = 0;

extern bool Flag_1ms;
unsigned char time_1ms=0,time_50ms=0;
unsigned char mode=0,mod=0;
unsigned int timec=0;
int last_encoder[4] = {0};
int Motor_speed[4]={0};
int set_Motor_speed = 0;
int set_Motor_PWM[4] = {0};
int rec_encoder=0;
#define move_left 4
#define move_right 5


unsigned char time_Catch=0;
char Func_Catch(){//抓取
	RELAYS3_ON();//抓取和放松有可能写反了 要测试一下
	time_Catch++;
	if(time_Catch>=20){
		time_Catch=0;
		return 0;
	}
	return 1;
}
unsigned char time_Release=0;
char Func_Release(){//放松
	RELAYS3_OFF();
	time_Release++;
	if(time_Release>=20){
		time_Release=0;
		return 0;
	}
	return 1;
}
char Func_UP(){//上升
	if (!HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin)){
		RELAYS2_OFF();
		RELAYS1_ON();
	}
	else {
		RELAYS1_OFF();
		return 0;
	}
	return 1;
}
char Func_DOWN(){//下降
	if (!HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin)){
		RELAYS1_OFF();
		RELAYS2_ON();
	}
	else {
		RELAYS2_OFF();
		return 0;
	}
	return 1;
}







/*
void move_(char dir,int len){

	if(dir==0){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==1){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==2){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==3){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==4){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==5){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}

}
static double   Proportion=0.15;                               //比例常数 Proportional Const
static double   Integral=0.1;                                 //积分常数 Integral Const
static double   Derivative=0;                                 //b不采用微分

//NowPoint当前输出值
//SetPoint设定值
int PID_Calc1(int NowPoint,int SetPoint)
{
                             //微分常数 Derivative Const
	static int      LastError1;                                //Error[-1]
	static int      PrevError1;                                //Error[-2]
  int iError,Outpid;                                   //当前误差

  iError=SetPoint-NowPoint;                           //增量计算
  Outpid=(Proportion * iError)                   //E[k]项
              -(Integral * LastError1)      //E[k-1]项
              +(Derivative * PrevError1);   //E[k-2]项

  PrevError1=LastError1;                     //存储误差，用于下次计算
  LastError1=iError;
  return(Outpid);                                      //返回增量值
}

int PID_Calc2(int NowPoint,int SetPoint)
{
                             //微分常数 Derivative Const
	static int      LastError2;                                //Error[-1]
	static int      PrevError2;                                //Error[-2]
  int iError,Outpid;                                   //当前误差

  iError=SetPoint-NowPoint;                           //增量计算
  Outpid=(Proportion * iError)                   //E[k]项
              -(Integral * LastError2)      //E[k-1]项
              +(Derivative * PrevError2);   //E[k-2]项

  PrevError2=LastError2;                     //存储误差，用于下次计算
  LastError2=iError;
  return(Outpid);                                      //返回增量值
}


int PID_Calc3(int NowPoint,int SetPoint)
{
                             //微分常数 Derivative Const
	static int      LastError3;                                //Error[-1]
	static int      PrevError3;                                //Error[-2]
  int iError,Outpid;                                   //当前误差

  iError=SetPoint-NowPoint;                           //增量计算
  Outpid=(Proportion * iError)                   //E[k]项
              -(Integral * LastError3)      //E[k-1]项
              +(Derivative * PrevError3);   //E[k-2]项

  PrevError3=LastError3;                     //存储误差，用于下次计算
  LastError3=iError;
  return(Outpid);                                      //返回增量值
}


int PID_Calc4(int NowPoint,int SetPoint)
{
                             //微分常数 Derivative Const
	static int      LastError4;                                //Error[-1]
	static int      PrevError4;                                //Error[-2]
  int iError,Outpid;                                   //当前误差

  iError=SetPoint-NowPoint;                           //增量计算
  Outpid=(Proportion * iError)                   //E[k]项
              -(Integral * LastError4)      //E[k-1]项
              +(Derivative * PrevError4);   //E[k-2]项

  PrevError4=LastError4;                     //存储误差，用于下次计算
  LastError4=iError;
  return(Outpid);                                      //返回增量值
}

int Encoder_Front_Left,Encoder_Front_Right,Encoder_Back_Right,Encoder_Back_Left;
int Left_t,Right_t,Encoder_R,Encoder_L;
int Moto_Front_Left,Moto_Front_Right,Moto_Back_Left,Moto_Back_Right;
int para1,para2,para3,para4;//增量
int npo[4]={0};
void Car_move_pid_4wpo(unsigned char dir,int len){
	if(dir==0){
		npo[0] = encoder[0] - len;
		npo[1] = encoder[1] - len;
		npo[2] = encoder[2] - len;
		npo[3] = encoder[3] - len;
	}
	else if(dir==1){
		npo[0] = encoder[0] + len;
		npo[1] = encoder[1] + len;
		npo[2] = encoder[2] - len;
		npo[3] = encoder[3] - len;
	}
	else if(dir==2){
		npo[0] = encoder[0] - len;
		npo[1] = encoder[1] - len;
		npo[2] = encoder[2] + len;
		npo[3] = encoder[3] + len;
	}
	else if(dir==3){
		npo[0] = encoder[0] + len;
		npo[1] = encoder[1] + len;
		npo[2] = encoder[2] + len;
		npo[3] = encoder[3] + len;
	}
	else if(dir==4){
		npo[0] = encoder[0] - len;
		npo[1] = encoder[1] + len;
		npo[2] = encoder[2] + len;
		npo[3] = encoder[3] - len;
	}
	else if(dir==5){
		npo[0] = encoder[0] + len;
		npo[1] = encoder[1] - len;
		npo[2] = encoder[2] - len;
		npo[3] = encoder[3] + len;
	}

	para1=PID_Calc1(encoder[0],npo[0]);	//左电机，计数得到增量式PID的增量数值
	para2=PID_Calc2(encoder[1],npo[1]);
	para3=PID_Calc3(encoder[2],npo[2]);
	para4=PID_Calc4(encoder[3],npo[3]);

	if((para1<-3)||(para1>3)) // 不做 PID 调整，避免误差较小时频繁调节引起震荡。
	{
		Moto_Front_Left +=para1;
	}
			if(Moto_Front_Left>3500) Moto_Front_Left=3500;//限幅
	    //TIM8->CCR1=Moto_Front_Left;//更新pwm

	if((para2<-3)||(para2>3)) // 不做 PID 调整，避免误差较小时频繁调节引起震荡。
	{
		Moto_Front_Right +=para2;
	}
			if(Moto_Front_Right>3500) Moto_Front_Right=3500;//限幅
	    //TIM8->CCR2=Moto_Front_Right;

///
	if((para3<-3)||(para3>3)) // 不做 PID 调整，避免误差较小时频繁调节引起震荡。
	{
		Moto_Back_Left +=para3;
	}
			if(Moto_Back_Left>3500) Moto_Back_Left=3500;//限幅
	    //TIM8->CCR3=Moto_Back_Left;//更新pwm

	if((para4<-3)||(para4>3)) // 不做 PID 调整，避免误差较小时频繁调节引起震荡。
	{
		Moto_Back_Right +=para4;
	}
			if(Moto_Back_Right>3500) Moto_Back_Right=3500;//限幅
	    //TIM8->CCR4=Moto_Back_Right;//更新pwm

	set_Motor_PWM[0] = Moto_Front_Left;
	set_Motor_PWM[1] = Moto_Front_Right;
	set_Motor_PWM[2] = Moto_Back_Left;
	set_Motor_PWM[3] = Moto_Back_Right;

	move_(dir,len);

}*/

























volatile double pid_l_e1_T=0,pid_Lspeedset_T=0,pid_l_speeddet_T=0;
volatile double pid_r_e1_T=0,pid_Rspeedset_T=0,pid_r_speeddet_T=0;
volatile double differ_e1_T=0,differ_integral_T=0,pid_l_integral_T=0,pid_r_integral_T=0,pid_l_duk_T=0,pid_r_duk_T=0,differ_e2_T=0;
volatile double pid_l_e2_T=0,pid_r_e2_T=0;
volatile double pid_l_e3_T=0,pid_r_e3_T=0;
volatile double leftwalkmotor_pwm_T=0,rightwalkmotor_pwm_T=0,pid_l_uk_T=0,pid_r_uk_T=0;


volatile double pid_l_e1=0,pid_Lspeedset=0,pid_l_speeddet=0;
volatile double pid_r_e1=0,pid_Rspeedset=0,pid_r_speeddet=0;
volatile double differ_e1=0,differ_integral=0,pid_l_integral=0,pid_r_integral=0,pid_l_duk=0,pid_r_duk=0,differ_e2=0;
volatile double pid_l_e2=0,pid_r_e2=0;
volatile double pid_l_e3=0,pid_r_e3=0;
volatile double leftwalkmotor_pwm=0,rightwalkmotor_pwm=0,pid_l_uk=0,pid_r_uk=0;
#define vu16 volatile unsigned int
#define vs16 volatile int
//3 0.01 0.3
void PID_reset(){
	pid_l_e1=0;pid_l_e2=0;pid_l_e3=0;
	pid_r_e1=0;pid_r_e2=0;pid_r_e3=0;
	pid_l_integral=0;pid_r_integral=0;
	pid_l_duk=0;pid_r_duk=0;pid_l_speeddet=0;pid_r_speeddet=0;
	pid_l_uk=0;pid_r_uk=0;

	differ_e1=0;differ_e2=0;
	differ_integral=0;


	pid_l_e1_T=0;pid_l_e2_T=0;pid_l_e3_T=0;
	pid_r_e1_T=0;pid_r_e2_T=0;pid_r_e3_T=0;
	pid_l_integral_T=0;pid_r_integral_T=0;
	pid_l_duk_T=0;pid_r_duk_T=0;pid_l_speeddet_T=0;pid_r_speeddet_T=0;
	pid_l_uk_T=0;pid_r_uk_T=0;

	differ_e1_T=0;differ_e2_T=0;
	differ_integral_T=0;

}
void Walkmotor_Pidcontrol2(float wkp,float wki,float wkd)//目前正在使用160711
{
	float le1ki_index,re1ki_index;
	//vs16 le1_integral,re1_integral;
	vu16 le1_abs,re1_abs,le1_max,le1_min,re1_max,re1_min;
	pid_l_e1_T=pid_Lspeedset_T-pid_l_speeddet_T;//设定值-测量值=当前误差
	pid_r_e1_T=pid_Rspeedset_T-pid_r_speeddet_T;//设定值-测量值=当前误差
	differ_e1_T=pid_l_speeddet_T-pid_r_speeddet_T;//测量左轮测量值-右轮测量值=当前误差
	differ_integral_T+=differ_e1_T;    //积分两轮的误差
	le1_max=(vu16)(pid_Lspeedset_T*0.85f);
	le1_min=(vu16)(pid_Lspeedset_T*0.15f);
	re1_max=(vu16)(pid_Rspeedset_T*0.85f);
	re1_min=(vu16)(pid_Rspeedset_T*0.15f);
// 	le1_max=(vu16)(pid_Lspeedset_T*0.8f);//暂时数据
// 	le1_min=(vu16)(pid_Lspeedset_T*0.2f);
// 	re1_max=(vu16)(pid_Rspeedset_T*0.8f);
// 	re1_min=(vu16)(pid_Rspeedset_T*0.2f);
	//求本次左轮偏差的绝对值
	if(pid_l_e1_T<0){le1_abs=pid_l_speeddet_T-pid_Lspeedset_T;}
	else{le1_abs=pid_l_e1_T;}
	//求本次右轮偏差的绝对值
	if(pid_r_e1_T<0){re1_abs=pid_r_speeddet_T-pid_Rspeedset_T;}
	else{re1_abs=pid_r_e1_T;}
	//求左轮积分系数
	//if(le1_abs>pid_Lspeedset){le1ki_index=0;}//误差较大，积分分离
	if(le1_abs>le1_max){le1ki_index=0;}//误差较大，积分分离
	else if(le1_abs<le1_min){le1ki_index=1;pid_l_integral_T+=pid_l_e1_T;}
	else{
		le1ki_index=(float)((le1_max-le1_abs)/(le1_max-le1_min));
		//le1ki_index=(float)((pid_Lspeedset-le1_abs)/(pid_Lspeedset-le1_min));
		pid_l_integral_T+=pid_l_e1_T;
	}
	//求右轮积分系数
	//if(re1_abs>pid_Rspeedset){re1ki_index=0;}
	if(re1_abs>re1_max){re1ki_index=0;}//误差较大，积分分离
	else if(re1_abs<re1_min){re1ki_index=1;pid_r_integral_T+=pid_r_e1_T;}
	else{
		re1ki_index=(float)((re1_max-re1_abs)/(re1_max-re1_min));
		//re1ki_index=(float)((pid_Rspeedset-re1_abs)/(pid_Rspeedset-re1_min));
		pid_r_integral_T+=pid_r_e1_T;
	}
	//计算左轮增量

	pid_l_duk_T=(vs16)(wkp*pid_l_e1_T+le1ki_index*wki*pid_l_integral_T+wkd*(pid_l_e1_T-pid_l_e2_T)/*-wkp*differ_e1_T-1.0f*differ_integral_T-1.0f*(differ_e1_T-differ_e2_T)*/);

	//计算左轮本次输出
	pid_l_uk_T=pid_l_uk_T+pid_l_duk_T;
	if(pid_l_uk_T>3500){pid_l_uk_T=3500;}
	//if(pid_l_uk>680){pid_l_uk=680;}
	else{
		if(pid_l_uk_T<0){pid_l_uk_T=0;}
		//if(pid_l_uk<0){pid_l_uk=2;}
	}
	//计算右轮增量

	pid_r_duk_T=(vs16)(wkp*pid_r_e1_T+re1ki_index*wki*pid_r_integral_T+wkd*(pid_r_e1_T-pid_r_e2_T)/*+wkp*differ_e1_T+1.0f*differ_integral_T+1.0f*(differ_e1_T-differ_e2_T)*/);
//	pid_r_duk=  (vs16)(wkp*pid_r_e1  +re1ki_index*wki*pid_r_integral  +wkd*(pid_r_e1  -pid_r_e2  )+wkp*differ_e1  +1.0f*differ_integral  +1.0f*(differ_e1  -differ_e2  ));

	//计算右轮本次输出
	pid_r_uk_T=pid_r_uk_T+pid_r_duk_T;
	if(pid_r_uk_T>3500){pid_r_uk_T=3500;}
	//if(pid_r_uk>680){pid_r_uk=680;}
	else{
		if(pid_r_uk_T<0){pid_r_uk_T=0;}
	  //if(pid_r_uk<0){pid_r_uk=2;}
	}
// 	leftwalkmotor_pwm=pid_l_uk*5;
// 	rightwalkmotor_pwm=pid_r_uk*5;
	leftwalkmotor_pwm_T=pid_l_uk_T;
	rightwalkmotor_pwm_T=pid_r_uk_T;
// 	leftwalkmotor_pwm=pid_l_uk*30;
// 	rightwalkmotor_pwm=pid_r_uk*30;
	pid_l_e3_T=pid_l_e2_T;pid_l_e2_T=pid_l_e1_T;
	pid_r_e3_T=pid_r_e2_T;pid_r_e2_T=pid_r_e1_T;
	differ_e2_T=differ_e1_T;//新添加
	/*TIM_SetCompare1(TIM1, leftwalkmotor_pwm);//左轮PWM设定,正转
	TIM_SetCompare2(TIM1, leftwalkmotor_pwm);//左轮PWM设定，反转
	TIM_SetCompare3(TIM1, rightwalkmotor_pwm);//右轮PWM设定,正转
	TIM_SetCompare4(TIM1, rightwalkmotor_pwm);//右轮PWM设定，反转*/
}
void Walkmotor_Pidcontrol(float wkp,float wki,float wkd)//目前正在使用160711
{
	float le1ki_index,re1ki_index;
	//vs16 le1_integral,re1_integral;
	vu16 le1_abs,re1_abs,le1_max,le1_min,re1_max,re1_min;
	pid_l_e1=pid_Lspeedset-pid_l_speeddet;//设定值-测量值=当前误差
	pid_r_e1=pid_Rspeedset-pid_r_speeddet;//设定值-测量值=当前误差
	differ_e1=pid_l_speeddet-pid_r_speeddet;//测量左轮测量值-右轮测量值=当前误差
	differ_integral+=differ_e1;    //积分两轮的误差
	le1_max=(vu16)(pid_Lspeedset*0.85f);
	le1_min=(vu16)(pid_Lspeedset*0.15f);
	re1_max=(vu16)(pid_Rspeedset*0.85f);
	re1_min=(vu16)(pid_Rspeedset*0.15f);
// 	le1_max=(vu16)(pid_Lspeedset*0.8f);//暂时数据
// 	le1_min=(vu16)(pid_Lspeedset*0.2f);
// 	re1_max=(vu16)(pid_Rspeedset*0.8f);
// 	re1_min=(vu16)(pid_Rspeedset*0.2f);
	//求本次左轮偏差的绝对值
	if(pid_l_e1<0){le1_abs=pid_l_speeddet-pid_Lspeedset;}
	else{le1_abs=pid_l_e1;}
	//求本次右轮偏差的绝对值
	if(pid_r_e1<0){re1_abs=pid_r_speeddet-pid_Rspeedset;}
	else{re1_abs=pid_r_e1;}
	//求左轮积分系数
	//if(le1_abs>pid_Lspeedset){le1ki_index=0;}//误差较大，积分分离
	if(le1_abs>le1_max){le1ki_index=0;}//误差较大，积分分离
	else if(le1_abs<le1_min){le1ki_index=1;pid_l_integral+=pid_l_e1;}
	else{
		le1ki_index=(float)((le1_max-le1_abs)/(le1_max-le1_min));
		//le1ki_index=(float)((pid_Lspeedset-le1_abs)/(pid_Lspeedset-le1_min));
		pid_l_integral+=pid_l_e1;
	}
	//求右轮积分系数
	//if(re1_abs>pid_Rspeedset){re1ki_index=0;}
	if(re1_abs>re1_max){re1ki_index=0;}//误差较大，积分分离
	else if(re1_abs<re1_min){re1ki_index=1;pid_r_integral+=pid_r_e1;}
	else{
		re1ki_index=(float)((re1_max-re1_abs)/(re1_max-re1_min));
		//re1ki_index=(float)((pid_Rspeedset-re1_abs)/(pid_Rspeedset-re1_min));
		pid_r_integral+=pid_r_e1;
	}
	//计算左轮增量

	pid_l_duk=(vs16)(wkp*pid_l_e1+le1ki_index*wki*pid_l_integral+wkd*(pid_l_e1-pid_l_e2)/*-wkp*differ_e1-1.0f*differ_integral-1.0f*(differ_e1-differ_e2)*/);

	//计算左轮本次输出
	pid_l_uk=pid_l_uk+pid_l_duk;
	if(pid_l_uk>3500){pid_l_uk=3500;}
	//if(pid_l_uk>680){pid_l_uk=680;}
	else{
		if(pid_l_uk<0){pid_l_uk=0;}
		//if(pid_l_uk<0){pid_l_uk=2;}
	}
	//计算右轮增量

	pid_r_duk=(vs16)(wkp*pid_r_e1+re1ki_index*wki*pid_r_integral+wkd*(pid_r_e1-pid_r_e2)/*-wkp*differ_e1-1.0f*differ_integral-1.0f*(differ_e1-differ_e2)*/);

	//计算右轮本次输出
	pid_r_uk=pid_r_uk+pid_r_duk;
	if(pid_r_uk>3500){pid_r_uk=3500;}
	//if(pid_r_uk>680){pid_r_uk=680;}
	else{
		if(pid_r_uk<0){pid_r_uk=0;}
	  //if(pid_r_uk<0){pid_r_uk=2;}
	}
// 	leftwalkmotor_pwm=pid_l_uk*5;
// 	rightwalkmotor_pwm=pid_r_uk*5;
	leftwalkmotor_pwm=pid_l_uk;
	rightwalkmotor_pwm=pid_r_uk;
// 	leftwalkmotor_pwm=pid_l_uk*30;
// 	rightwalkmotor_pwm=pid_r_uk*30;
	pid_l_e3=pid_l_e2;pid_l_e2=pid_l_e1;
	pid_r_e3=pid_r_e2;pid_r_e2=pid_r_e1;
	differ_e2=differ_e1;//新添加
	/*TIM_SetCompare1(TIM1, leftwalkmotor_pwm);//左轮PWM设定,正转
	TIM_SetCompare2(TIM1, leftwalkmotor_pwm);//左轮PWM设定，反转
	TIM_SetCompare3(TIM1, rightwalkmotor_pwm);//右轮PWM设定,正转
	TIM_SetCompare4(TIM1, rightwalkmotor_pwm);//右轮PWM设定，反转*/
}
void Motor_speed_det(){
	Encoder_Update_Count();
	Encoder_Get_ALL(encoder);//读编码器数值

	Motor_speed[0] = abs(last_encoder[0]-encoder[0]);
	Motor_speed[1] = abs(last_encoder[1]-encoder[1]);
	Motor_speed[2] = abs(last_encoder[2]-encoder[2]);
	Motor_speed[3] = abs(last_encoder[3]-encoder[3]);

	last_encoder[0] = encoder[0];
	last_encoder[1] = encoder[1];
	last_encoder[2] = encoder[2];
	last_encoder[3] = encoder[3];
}
#define Motor_speed_ 50
char Car_move_pid(char dir,int len){
	Motor_speed_det();

	pid_Lspeedset = Motor_speed_;
	pid_Rspeedset = Motor_speed_;
	pid_l_speeddet = Motor_speed[1];
	pid_r_speeddet = Motor_speed[2];

	Walkmotor_Pidcontrol( 3 , 0.01 , 0 );

	set_Motor_PWM[1] = leftwalkmotor_pwm;
	set_Motor_PWM[2] = rightwalkmotor_pwm;


	pid_Lspeedset_T = Motor_speed_;
	pid_Rspeedset_T = Motor_speed_;
	pid_l_speeddet_T = Motor_speed[0];
	pid_r_speeddet_T = Motor_speed[3];

	Walkmotor_Pidcontrol2( 3 , 0.01 , 0 );

	set_Motor_PWM[0] = leftwalkmotor_pwm_T;
	set_Motor_PWM[3] = rightwalkmotor_pwm_T;

	if(dir==0){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==1){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==2){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==3){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==4){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==5){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	Run_C = 1;

	if(abs(encoder[0]-rec_encoder)>=len /*|| abs(encoder[1]-rec_encoder)>=len || abs(encoder[2]-rec_encoder)>=len || abs(encoder[3]-rec_encoder)>=len*/){
		Motion_Ctrl(0,0,0);
		Motor_Stop(1);		//停止
		PID_reset();
		HAL_Delay(500);
		Encoder_Update_Count();
		Encoder_Get_ALL(encoder);//读编码器数值

		rec_encoder = encoder[0];
		return 0;
	}

	return 1;
}
char Car_move_no_pid(char dir,int len){
	Motor_speed_det();

	set_Motor_speed=50;
	if(Motor_speed[0]<set_Motor_speed){
		set_Motor_PWM[0] += 10;
	}
	else if(Motor_speed[0]>set_Motor_speed){
		set_Motor_PWM[0] -= 10;
	}

	if(Motor_speed[1]<set_Motor_speed){
		set_Motor_PWM[1] += 10;
	}
	else if(Motor_speed[0]>set_Motor_speed){
		set_Motor_PWM[1] -= 10;
	}

	if(Motor_speed[2]<set_Motor_speed){
		set_Motor_PWM[2] += 10;
	}
	else if(Motor_speed[0]>set_Motor_speed){
		set_Motor_PWM[2] -= 10;
	}

	if(Motor_speed[3]<set_Motor_speed){
		set_Motor_PWM[3] += 10;
	}
	else if(Motor_speed[0]>set_Motor_speed){
		set_Motor_PWM[3] -= 10;
	}


	if(dir==0){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==1){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==2){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==3){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==4){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==5){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	Run_C = 1;

	if(abs(encoder[0]-rec_encoder)>=len /*|| abs(encoder[1]-rec_encoder)>=len || abs(encoder[2]-rec_encoder)>=len || abs(encoder[3]-rec_encoder)>=len*/){
		Motion_Ctrl(0,0,0);
		Motor_Stop(1);		//停止
		PID_reset();
		HAL_Delay(500);
		Motor_speed_det();

		rec_encoder = encoder[0];
		return 0;
	}

	return 1;
}
char Car_move(char dir,int len){
	if(dir==0){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==1){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==2){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==3){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}
	else if(dir==4){
		Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);
	}
	else if(dir==5){
		Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
		Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
		Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
		Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);
	}

	Run_C = 1;

	if(abs(encoder[0]-rec_encoder)>=len){
		Motion_Ctrl(0,0,0);
		Motor_Stop(1);		//停止

		rec_encoder = encoder[0];
		return 0;
	}
	return 1;
}
void Car_stop(){
	Motion_Ctrl(0,0,0);
	Motor_Stop(1);		//停止

	rec_encoder = encoder[0];
}
void Motor_ctrl(){//1ms
	if(mode==0){
		if(mod==0){
			Motion_Ctrl(0,0,0);
			Motor_Stop(1);		//停止
			mod=1;
		}
		else if(mod==1){
			;
		}
	}
	else if(mode==1){
		if(mod==0){
			set_Motor_speed=4;/*
			if(Motor_speed[0]<set_Motor_speed){
				set_Motor_PWM[0] += 1;
			}
			else if(Motor_speed[0]>set_Motor_speed){
				set_Motor_PWM[0] -= 1;
			}

			if(Motor_speed[1]<set_Motor_speed){
				set_Motor_PWM[1] += 1;
			}
			else if(Motor_speed[0]>set_Motor_speed){
				set_Motor_PWM[1] -= 1;
			}

			if(Motor_speed[2]<set_Motor_speed){
				set_Motor_PWM[2] += 1;
			}
			else if(Motor_speed[0]>set_Motor_speed){
				set_Motor_PWM[2] -= 1;
			}

			if(Motor_speed[3]<set_Motor_speed){
				set_Motor_PWM[3] += 1;
			}
			else if(Motor_speed[0]>set_Motor_speed){
				set_Motor_PWM[3] -= 1;
			}*/


			Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
			Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
			Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
			Motor_Set_Pwm(MOTOR_ID_M4, -set_Motor_PWM[3]);

			Run_C = 1;

			if(abs(encoder[0])>=3490){
				Motion_Ctrl(0,0,0);
				Motor_Stop(1);		//停止

				//encoder[0]=encoder[1]=encoder[2]=encoder[3]=0;
				rec_encoder = encoder[0];
				mod=1;
			}
		}
		else if(mod==1){
			timec++;
			if(timec>=100){
				timec=0;

				rec_encoder = encoder[0];
				mod=2;
			}
		}
		else if(mod==2){
			Motor_Set_Pwm(MOTOR_ID_M1, set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
			Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
			Motor_Set_Pwm(MOTOR_ID_M3, -set_Motor_PWM[2]);
			Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);

			Run_C = 1;

			if(abs(encoder[0]-rec_encoder)>=1000){
				Motion_Ctrl(0,0,0);
				Motor_Stop(1);		//停止

				//encoder[0]=encoder[1]=encoder[2]=encoder[3]=0;
				rec_encoder = encoder[0];
				mod=3;
			}
		}
		else if(mod==3){
			timec++;
			if(timec>=100){
				timec=0;

				rec_encoder = encoder[0];
				mod=4;
			}
		}
		else if(mod==4){
			Motor_Set_Pwm(MOTOR_ID_M1, -set_Motor_PWM[0]);	//对四个电机进行向前行走赋值，这个电机慢所以要加快一点
			Motor_Set_Pwm(MOTOR_ID_M2, -set_Motor_PWM[1]);
			Motor_Set_Pwm(MOTOR_ID_M3, set_Motor_PWM[2]);
			Motor_Set_Pwm(MOTOR_ID_M4, set_Motor_PWM[3]);

			Run_C = 1;

			if(abs(encoder[0]-rec_encoder)>=1000){
				Motion_Ctrl(0,0,0);
				Motor_Stop(1);		//停止

				//encoder[0]=encoder[1]=encoder[2]=encoder[3]=0;
				rec_encoder = encoder[0];
				mod=5;
			}
		}
	}
}
void Func_work(){
	if(mode==0){
		if(mod==0){
			Car_stop();
			mod=1;
		}
		else if(mod==1){
			mod=2;
		}
		else if(mod==2){
			;
		}
	}
	else if(mode==1){
		if(mod==0){
			JY61P_Z_reset();
			//Beep_On_Time(50);
			//Car_stop();
			//if(!Car_move_no_pid(Forward,3500)){mod=100;HAL_Delay(1000);}
			if(!Car_move_pid(Forward,3500)){mod=100;HAL_Delay(1000);}
			mod=100;
		}
		else if(mod==1){
			if(!Car_move_pid(Forward,3500)){mod++;HAL_Delay(1000);}
			//Car_move_pid_4wpo(0,10000);
		}
		else if(mod==2){
			if(!Car_move_pid(Backward,50)){mod++;HAL_Delay(1000);}
		}
		else if(mod==3){
			if(!Car_move_pid(Left_turn,960)){mod++;HAL_Delay(1000);}
		}
		else if(mod==4){
			if(!Car_move_pid(Forward,2850)){mod++;HAL_Delay(1000);}
		}
		else if(mod==5){
			if(!Car_move_pid(Right_turn,960/2+5)){mod++;HAL_Delay(1000);}
		}
		else if(mod==6){
			if(!Car_move_pid(Left,320)){mod++;HAL_Delay(1000);}
		}
		else if(mod==7){
			if(!Car_move_pid(Left,320)){mod++;HAL_Delay(1000);}
		}
		else if(mod==8){
			if(!Car_move_pid(Left_turn,960/2+5)){mod++;HAL_Delay(1000);}
		}
		else if(mod==9){
			if(!Car_move_pid(Left,500)){mod++;HAL_Delay(1000);}
		}
	}
}

void Func_1ms(){
	Encoder_Update_Count();
	Encoder_Get_ALL(encoder);//读编码器数值

	Motor_speed[0] = abs(last_encoder[0]-encoder[0]);
	Motor_speed[1] = abs(last_encoder[1]-encoder[1]);
	Motor_speed[2] = abs(last_encoder[2]-encoder[2]);
	Motor_speed[3] = abs(last_encoder[3]-encoder[3]);

	last_encoder[0] = encoder[0];
	last_encoder[1] = encoder[1];
	last_encoder[2] = encoder[2];
	last_encoder[3] = encoder[3];

	//Motor_ctrl();
	Func_work();
}
void Func_10ms(){
	Encoder_Update_Count();
	Encoder_Get_ALL(encoder);//读编码器数值

	Bsp_Led_Show_State_Handle();
	Beep_Timeout_Close_Handle();
	if (Key0_State(KEY_MODE_ONE_TIME)){
		if(mode==0){
			mode=1,mod=0;
		}
		else mode=0,mod=0;
	}


	show_encoder++;
	if (show_encoder > 20)
	{
		show_encoder = 0;/*
//		Encoder_Get_ALL(encoder);
		printf(" Encoder:%d, %d, %d, %d\r\n", encoder[0], encoder[1], encoder[2], encoder[3]);
//	    printf(" Test-Data1:%d, %d, %d, %d\r\n", g_speed_L1, g_speed_L2, g_speed_R1, g_speed_R2);
	    printf(" Motor_speed:%d, %d, %d, %d\r\n", Motor_speed[0], Motor_speed[1], Motor_speed[2], Motor_speed[3]);
//	    printf(" last_encoder:%d, %d, %d, %d\r\n", last_encoder[0], last_encoder[1], last_encoder[2], last_encoder[3]);
	    printf(" set_Motor_PWM:%d, %d, %d, %d\r\n", set_Motor_PWM[0], set_Motor_PWM[1], set_Motor_PWM[2], set_Motor_PWM[3]);
	    printf(" YawZ:%f\r\n", YawZ);
	    printf("\r\n");*/
	}
}
// main.c中循环调用此函数，避免多次修改main.c文件。
// This function is called in a loop in main.c to avoid multiple modifications to the main.c file
void Bsp_Loop(void)
{
	//854, 637, 663, 637  speed=2
	//1754, 1023, 1066, 1009	speed=4
	/*set_Motor_PWM[0]=3702;
	set_Motor_PWM[1]=1102;
	set_Motor_PWM[2]=1129;
	set_Motor_PWM[3]=1061;*/
	set_Motor_PWM[0]=0;
	set_Motor_PWM[1]=0;
	set_Motor_PWM[2]=0;
	set_Motor_PWM[3]=0;
	//set_Motor_PWM[0]=set_Motor_PWM[1]=set_Motor_PWM[2]=set_Motor_PWM[3]=0;
	while(1){
		HAL_Delay(1);
		Flag_1ms = 1;
		if(Flag_1ms){
			Flag_1ms = 0;
			time_1ms++;
			//Func_1ms();
			if(time_1ms>=10){time_1ms=0;
				Func_10ms();
			}
			time_50ms++;
			if(time_50ms>=25){
				time_50ms=0;
				Func_work();
			}
		}
	}

	//RELAYS1_ON();		//抓取电机向上运行
	//RELAYS1_OFF();	//抓取电机向上运行停止
	//RELAYS2_ON();		//抓取电机向下运行
	//RELAYS2_OFF();	//抓取电机向下运行停止
	//RELAYS3_ON();		//抓取放 电磁阀，绿色连接器接口
	//RELAYS3_OFF();
	//if(0 == HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin))	//下限位 NPN传感器(绿色连接器接口)
	//if(0 == HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin))	//上限位 NPN传感器(黄色连接器接口)


	/*测试输入端口示例*/
	//	if (!HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin))
	//	{
	//		RELAYS2_OFF();
	//		RELAYS1_ON();
	//	}
	//	else RELAYS1_OFF();
	//	if (!HAL_GPIO_ReadPin(KEY2_GPIO_Port, KEY2_Pin))
	//	{
	//		RELAYS1_OFF();
	//		RELAYS2_ON();
	//	}
	//	else RELAYS2_OFF();

}

